import numpy as np
"""
OneEuroFilter类是一个用于低通滤波的Python实现。它通过计算加权平均值来平滑输入信号，从而减少噪声和抖动。

参数	含义	调整建议
te	采样周期（秒）	使用真实帧间隔，如 0.033 或 0.016
mincutoff	最小滤波频率	越小越灵敏，越大越平滑
beta	对速度变化的敏感性	越大越动态响应快
dcutoff	派生速度的滤波频率	通常保持默认 0.5
"""

class OneEuroFilter:
    def __init__(self, te, mincutoff=0.1, beta=0.007, dcutoff=0.5):
        self.x = None
        self.dx = 0
        self.te = te
        self.mincutoff = mincutoff
        self.beta = beta
        self.dcutoff = dcutoff
        self.alpha = self._alpha(self.mincutoff)
        self.dalpha = self._alpha(self.dcutoff)

    def _alpha(self, cutoff):
        tau = 1.0 / (2 * np.pi * cutoff)
        return 1.0 / (1.0 + tau / self.te)

    def __call__(self, x, te):
        result = x
        if self.x is not None:
            edx = (x - self.x) / te
            self.dx = self.dx + (self.dalpha * (edx - self.dx))
            cutoff = self.mincutoff + self.beta * abs(self.dx)
            self.alpha = self._alpha(cutoff)
            result = self.x + self.alpha * (x - self.x)
        self.x = result
        return result
class EMAFilter:
    def __init__(self, alpha=0.3):
        self.alpha = alpha
        self.prev = None

    def __call__(self, x):
        if self.prev is None:
            self.prev = x.copy()
            return x
        self.prev = self.alpha * x + (1 - self.alpha) * self.prev
        return self.prev
if __name__ == '__main__':
    filter = OneEuroFilter(te=0.01)
    points2d = np.random.randint(0, 100, (32, 2))
    import time
    t1 = time.time()
    for i in range(1000):
        points2d = filter(points2d, 0.01)
    t2 = time.time()
    print(t2-t1)
    # print(points2d)
    
    
    # filter = OneEuroFilter(te=0.01)
    # points3d = np.random.randint(0, 100, (32, 3))
    # points3d = filter(points3d, 0.01)
    # print(points3d)